package simpleGeom;

public class Vec3D extends Vec2D implements HasPos3D{

public float z=0;


public Vec3D(float x,float y,float z){
	this.x=x;
	this.y=y;
	this.z=z;
}
public Vec3D(double x,double y,double z){
	this.x=(float)x;
	this.y=(float)y;
	this.z=(float)z;
}
public Vec3D(float x,float y){
	this.x=x;
	this.y=y;
}

public Vec3D(){
}

public Vec3D(HasPos3D v){
	this.x=v.getX();
	this.y=v.getY();
	this.z=v.getZ();
}
public Vec3D(HasPos2D v){
	this.x=v.getX();
	this.y=v.getY();
	
}
public boolean equalPosition3D(Vec3D v,float dist){
	if (this.dist(v)<dist)return true;
	return false;
}
public boolean equalPosition(Vec3D v){
	if (v.x==x&&v.y==y&&v.z==z)return true;
	return false;
}

public void setPosition(float x,float y,float z){
	this.x=x;
	this.y=y;
	this.z=z;
}
public void setPosition(HasPos3D pos){
	setPosition(pos.getX(),pos.getY(),pos.getZ());
}

public Vec3D getCopy(){
	return new Vec3D(x,y,z);
}

public Vec3D add(HasPos3D v){
	this.x+=v.getX();
	this.y+=v.getY();
	this.z+=v.getZ();
	return this;
}

public void sub(HasPos3D v){
	this.x-=v.getX();
	this.y-=v.getY();
	this.z-=v.getZ();
}

public float mag(){
	return (float)Math.sqrt(magSqr());
}

public float magSqr(){
	return x*x+y*y+z*z;
}

public void mult(float f){
	
	this.x*=f;
	this.y*=f;
	this.z*=f;
}

public Vec3D scale(float f){
	 this.x*=f;
	this.y*=f;
	this.z*=f;
	return this;
}
public Vec3D getScaled(float f){
	return new Vec3D(x*f,y*f,z*f);
}

public void div(float f){
	this.x/=f;
	this.y/=f;
	this.z/=f;
}

public float distSqr(float x,float y,float z){
	float dX=this.x-x;
	float dY=this.y-y;
	float dZ=this.z-z;
	return dX*dX+dY*dY+dZ*dZ;
}

public float distSqr(HasPos3D v){
	return distSqr(v.getX(),v.getY(),v.getZ());
}

public float dist(float x,float y,float z){
	return (float)Math.sqrt(distSqr(x,y,z));
}



public float dist(HasPos3D v){
	return (float)Math.sqrt(distSqr(v));
}
public float dot(HasPos3D b) {
	return x * b.getX() + y * b.getY() + z * b.getZ();
}
public float dot(float cX,float cY,float cZ) {
	return x * cX + y * cY + z * cZ;
}
public Vec3D cross(HasPos3D b) {
	float cX = y * b.getZ() - z * b.getY();
	float cY = z * b.getX() - x * b.getZ();
	float cZ = x * b.getY() - y * b.getX();
	return new Vec3D(cX,cY,cZ);
}
public Vec3D crossSelf(HasPos3D b) {
	float nx = y * b.getZ() - z * b.getY();
	float ny = z * b.getX() - x * b.getZ();
	float nz= x * b.getY() - y * b.getX();
	x=nx;
	y=ny;
	z=nz;
	return this;
}
public static Vec3D cross(HasPos3D a, HasPos3D b) {
	float x = a.getY() * b.getZ() - a.getZ() * b.getY();
	float y = a.getZ() * b.getX() - a.getX() * b.getZ();
	float z = a.getX() * b.getY() - a.getY() * b.getX();
	return new Vec3D(x, y, z);
}
public void normalize(){
	div(mag());
}
public void invert(){
	mult(-1);
}
@Override
public void setLength(float l) {
	// TODO Auto-generated method stub
	this.mult(l/mag());
}
@Override
public String toString() {
	// TODO Auto-generated method stub
	return super.toString()+" x: "+x+" y: "+y+" z: "+z;
}

public float getAngleY(){
	float l2D=(float)Math.sqrt(x*x+y*y);
	return (float)Math.atan2(l2D, z);
}

public float getAngleZZ(){
	return (float)Math.atan2(y, x);
}

	float getYAngle(float x1, float y1, float z1, float x2, float y2, float z2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		float dZ = z2 - z1;
		float dXY = (float) Math.sqrt(dX * dX + dY * dY);
		return (float) Math.atan2(dZ, -dXY);
	}

	float getZAngle(float x1, float y1, float x2, float y2) {
		float dX = x2 - x1;
		float dY = y2 - y1;
		return (float) Math.atan2(dY, dX);
	}

public float getZ() {
	return z;
}

public Vec3D getOrthoVector(){
	// one of endless;
	Vec3D v=null;
	if (z!=0) v=new Vec3D(1,1,-(x+y)/z);
	else if (y!=0) v=new Vec3D(1,-(x+z)/y,1);
	else if (x!=0) v=new Vec3D(-(y+z)/x,1,1);
	v.normalize();
	return v;
}





public void setZ(float z) {
	this.z = z;
}

public static float dot(HasPos3D a,HasPos3D b) {
	return a.getX()* b.getX() + a.getY() * b.getY() + a.getZ() * b.getZ();
}
public static float mag(HasPos3D v){
	return (float)Math.sqrt(magSqr(v));
}
public static float magSqr(HasPos3D v){
	return  v.getX()* v.getX()+ v.getY()*v.getY()+v.getZ()*v.getZ();
}
public static Vec3D div(HasPos3D p, float f){
	return new Vec3D(p.getX()/f,p.getY()/f,p.getZ()/f);
}
public static float distSqr(HasPos3D v1,HasPos3D v2){
	float dX=v1.getX()-v2.getX();
	float dY=v1.getY()-v2.getY();
	float dZ=v1.getZ()-v2.getZ();
	
	return dX*dX+dY*dY+dZ*dZ;
}
public static float dist(float x1,float y1,float z1,float x2,float y2,float z2){
	return (float)Math.sqrt(distSqr(x1,y1,z1,x2,y2,z2));
}
public static float distSqr(float x1,float y1,float z1,float x2,float y2,float z2){
	float dX=x2-x1;
	float dY=y2-y1;
	float dZ=z2-z1;
	
	return dX*dX+dY*dY+dZ*dZ;
}
public static float dist(HasPos3D v1,HasPos3D v2){
	float d=distSqr(v1,v2);
	return (float)Math.sqrt(d);
}
public static float dot3D(HasPos3D a, HasPos3D b) {
	return a.getX() * b.getX() + a.getY() * b.getY() + a.getZ() * b.getZ();
}
public static Vec3D normalize(HasPos3D p){
	return  div(p,mag(p));
}
public static Vec3D getNormal(Vec3D a,Vec3D b,Vec3D c){
	Vec3D ab=Vec3D.sub(b,a);
	Vec3D ac=Vec3D.sub(c,a);
	Vec3D cross=Vec3D.cross(ac,ab);
	cross.normalize();
	return cross;
}
public static Vec3D getPolar3D(float zAngle,float xAngle,float l){
	Vec2D v1=Vec2D.getPolar(zAngle, 1);
	float cZ=v1.getX();
	float cY=v1.getY();
	Vec2D v2=Vec2D.getPolar(xAngle, cY);
	Vec3D v=new Vec3D(v2.getX(),v2.getY(),cZ);
	v.setLength(l);
	return v;
}
public void rotateAroundLine(HasPos3D o,HasPos3D vec,float angle){
	Vec3D v=rotateAroundLine(this,o,vec,angle);
	 
	this.setPosition(v.x, v.y, v.z);
}



public static Vec3D rotateAroundLine(HasPos3D p,HasPos3D o,HasPos3D vec,float angle){
	//http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/
	//If we multiply this times ⟨x,y,z⟩ we can obtain a function of of ten variables that yields the result of 
	//rotating the point (x,y,z) about the line through (a,b,c) with direction vector ⟨u,v,w⟩ (where u2 + v2 + w2 = 1) by the angle θ.
	float x=p.getX();
	float y=p.getY();
	float z=p.getZ();
	float a=o.getX();
	float b=o.getY();
	float c=o.getZ();
	float u=vec.getX();
	float v=vec.getY();
	float w=vec.getZ();
	float cos=(float)Math.cos(angle);
	float sin=(float)Math.sin(angle);
	float newX=(a*(v*v+w*w)-u*(b*v+c*w-u*x-v*y-w*z))*(1-cos)+x*cos+(-c*v+b*w-w*y+v*z)*sin;
	float newY=(b*(u*u+w*w)-v*(a*u-c*w-u*x-v*y-w*z))*(1-cos)+y*cos+(c*u-a*w+w*x-u*z)*sin;
	float newZ=(c*(u*u+v*v)-w*(a*u+b*v-u*x-v*y-w*z))*(1-cos)+z*cos+(-b*u+a*v-v*x+u*y)*sin;
	return new Vec3D(newX,newY,newZ);
}

public static Vec3D mult(HasPos3D p1,float m){
	
	return new Vec3D(p1.getX()*m,p1.getY()*m,p1.getZ()*m);
}
public static Vec3D add(HasPos3D v1,HasPos3D v2){
	return new Vec3D(v1.getX()+v2.getX(),v1.getY()+v2.getY(),v1.getZ()+v2.getZ());
}
public Vec3D getAdd(Vec3D b){
	return add(this,b);
}
public static Vec3D sub(HasPos3D v1,HasPos3D v2){
	
	return new Vec3D(v1.getX()-v2.getX(),v1.getY()-v2.getY(),v1.getZ()-v2.getZ());
}
public Vec3D getSub(Vec3D b){
	return sub(this,b);
}
public static Vec3D getNormal(HasPos3D p1, HasPos3D p2, HasPos3D p3) {
	Vec3D v1 = Vec3D.sub(p2, p1);
	Vec3D v2 = Vec3D.sub(p3, p1);
	return v1.cross(v2);
}

public static Vec3D getBetween(HasPos3D v1,HasPos3D v2,float f){
	float cX=f*(v2.getX()-v1.getX())+v1.getX();
	float cY=f*(v2.getY()-v1.getY())+v1.getY();
	float cZ=f*(v2.getZ()-v1.getZ())+v1.getZ();
	return new Vec3D(cX,cY,cZ);
}

}
